#pragma config(Hubs,  S1, HTMotor,  HTMotor,  none,     none)
#pragma config(Hubs,  S2, HTServo,  HTMotor,  none,     none)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#pragma config(Sensor, S2,     ,               sensorI2CMuxController)
#pragma config(Motor,  mtr_S1_C1_1,     rightFront,    tmotorTetrix, PIDControl, reversed, encoder)
#pragma config(Motor,  mtr_S1_C1_2,     rightBack,     tmotorTetrix, PIDControl, reversed, encoder)
#pragma config(Motor,  mtr_S1_C2_1,     leftBack,      tmotorTetrix, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C2_2,     leftFront,     tmotorTetrix, PIDControl, encoder)
#pragma config(Motor,  mtr_S2_C2_1,     beaterBar,     tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S2_C2_2,     lift,          tmotorTetrix, openLoop, encoder)
#pragma config(Servo,  srvo_S2_C1_1,    backRightServo,       tServoStandard)
#pragma config(Servo,  srvo_S2_C1_2,    backLeftServo,        tServoStandard)
#pragma config(Servo,  srvo_S2_C1_3,    rightServo,           tServoNone)
#pragma config(Servo,  srvo_S2_C1_4,    leftServo,            tServoNone)
#pragma config(Servo,  srvo_S2_C1_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S2_C1_6,    servo6,               tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//
#include "JoystickDriver.c"

float zero = 25;

void ySet(short speed){
	motor[leftFront] = speed;
	motor[leftBack] = speed;
	motor[rightFront] = speed;
	motor[rightBack] = speed;
}
task main()
{
	nMotorEncoder[leftBack] = 0;
	nMotorEncoder[rightBack] = 0;
	nMotorEncoder[leftFront] = 0;
	nMotorEncoder[rightFront] = 0;

	while(true){
		nxtDisplayTextLine(2, "leftBack: %3.0f", nMotorEncoder[leftBack]/4);
		nxtDisplayTextLine(3, "leftFront: %3.0f", nMotorEncoder[leftFront]/4);
		nxtDisplayTextLine(4, "rightBack: %3.0f", nMotorEncoder[rightBack]/4);
		nxtDisplayTextLine(5, "rightFront: %3.0f", nMotorEncoder[rightFront]/4);



			if(nNxtButtonPressed == 1){
				ySet(30);
			}else if(nNxtButtonPressed == 2){
				ySet(-30);
			}else{
				ySet(0);
			}


	}


}
